#include "FeatureDescriptor.h"

FeatureDescriptor::FeatureDescriptor(){descriptor = 0; descriptor_length = 0; type = -1;};

FeatureDescriptor::FeatureDescriptor(float * feature_descriptor, unsigned int feature_length){
	descriptor = 0;
	descriptor = feature_descriptor;
	descriptor_length = feature_length;
	type = 0;
	
};

FeatureDescriptor::FeatureDescriptor(unsigned int feature_length){
	descriptor = 0;
	descriptor = new float[feature_length];
	for(unsigned int i = 0; i < feature_length;i++){descriptor[i] = 0;}
	descriptor_length = feature_length;
	type = 0;
};

FeatureDescriptor::~FeatureDescriptor(){
	delete descriptor;
	descriptor = 0;
	descriptor_length = 0;
};

void FeatureDescriptor::print()
{
	ROS_INFO("Descriptor type: %i",type);
	for(int i = 0; i < descriptor_length; i++)
	{
		ROS_INFO("val[%i] = %f",i,descriptor[i]);
	}
}

inline double FeatureDescriptor::distance(FeatureDescriptor * other_descriptor)
{
	if(type != other_descriptor->type){return 999999;}
	else
	{
		double dist = 0;
		float tmp;
		float * disc = other_descriptor->descriptor;
		for(unsigned int i = 0; i < descriptor_length; i++){
			tmp 	= descriptor[i] - disc[i];
			dist	+= tmp*tmp;
		}
		return dist;
	}
}

inline FeatureDescriptor * FeatureDescriptor::add(FeatureDescriptor * feature)
{
	float * desc = feature->descriptor;
	for(unsigned int i = 0; i < descriptor_length; i++){descriptor[i]+=desc[i];}
	return this;
}

inline FeatureDescriptor * FeatureDescriptor::mul(float val)
{
	for(unsigned int i = 0; i < descriptor_length; i++){descriptor[i]*=val;}
	return this;
}

void FeatureDescriptor::normalize()
{
	float sum = 0;
	for(int i = 0; i < descriptor_length; i++)
	{
		sum += descriptor[i];
	}
	float div = 1/sum;
	for(int i = 0; i < descriptor_length; i++)
	{
		descriptor[i]*=div;
	}
}
